package anyVehicle;

import javax.vecmath.Vector3f;

import net.minecraft.src.Entity;

import com.bulletphysics.collision.dispatch.CollisionFlags;
import com.bulletphysics.collision.shapes.CollisionShape;
import com.bulletphysics.linearmath.Transform;

public class KinematicBodyUtils {
	private static Vector3f tmpVector = new Vector3f();

	
	public void updateEntityFromKinematicBody(Entity entity, EntityRigidBody body) {
		Transform t = new Transform();
		body.getMotionState().getWorldTransform(t);
		entity.setPosition(VehicleUtil.assignIfDifferenceIsGreaterThan(1e-4, t.origin.x, entity.posX),
				VehicleUtil.assignIfDifferenceIsGreaterThan(1e-4, t.origin.y + entity.yOffset - entity.ySize - 1f, entity.posY),
				VehicleUtil.assignIfDifferenceIsGreaterThan(1e-4, t.origin.z, entity.posZ));
	}

//	public EntityRigidBody createEntityKinematicBody(Entity entity, CollisionShape shape) {
//		// Create Dynamic Objects
//		Transform startTransform = new Transform();
//		startTransform.setIdentity();
//
//		float mass = 1f;
//
//		startTransform.origin.set((float) entity.posX, (float) entity.posY, (float) entity.posZ);
//
//		EntityRigidBody rb = createBody(mass, startTransform, shape, true);
//
//		rb.setAngularFactor(0F); // to avoid capsule rotation
//
//		rb.setCollisionFlags(rb.getCollisionFlags() | CollisionFlags.NO_CONTACT_RESPONSE | CollisionFlags.KINEMATIC_OBJECT);
//
//		rb.setUserPointer(entity);
//		return rb;
//	}
	public void translateKinematicBody(EntityRigidBody body, float dx, float dy, float dz) {
		Transform t = new Transform();
		body.getMotionState().getWorldTransform(t);
		t.origin.x += dx;
		t.origin.y += dy;
		t.origin.z += dz;
		body.getMotionState().setWorldTransform(t);
	}

	public void translateKinematicBody(EntityRigidBody body, Vector3f translation) {
		translateKinematicBody(body, translation.x, translation.y, translation.z);
	}

	public void setKinematicBodyPosition(EntityRigidBody body, float x, float y, float z) {
		Transform t = new Transform();
		body.getMotionState().getWorldTransform(t);
		t.origin.x = x;
		t.origin.y = y;
		t.origin.z = z;
		body.getMotionState().setWorldTransform(t);
	}

	public void kinematicToRigid(EntityRigidBody body, float mass) {
		body.getCollisionShape().calculateLocalInertia(mass, tmpVector );
		body.setMassProps(mass, tmpVector);
		body.setCollisionFlags(body.getCollisionFlags() & ~CollisionFlags.KINEMATIC_OBJECT);

	}

	public void rigidToKinematic(EntityRigidBody body) {
		tmpVector.set(0f, 0f, 0f);
		body.setMassProps(0f, tmpVector);
		body.setCollisionFlags(body.getCollisionFlags() | CollisionFlags.KINEMATIC_OBJECT);

	}
}
